package require critcl
source ../../TMD.tcl
critcl::ccode {
    #include "math.h"
    #include <stdlib.h>
    #include "stdio.h"
    const double PI=3.1415926535897932384626433832795;
    double normaldistr(double phi,double k1,double c1,double u1) {
	phi=phi*PI/180;
	phi=cos(c1*(phi-u1));
	phi=phi*phi;
        return exp(-phi/(2*k1*k1))/(k1*2.506628253);
    }
    double bessei0(double x) {
        double ax,ans;
        double y;
       if ((ax=fabs(x)) < 3.75) {
         //Polynomial fit.
          y=x/3.75;
          y*=y;
          ans=1.0+y*(3.5156229+y*(3.0899424+y*(1.2067492+y*(0.2659732+y*(0.360768e-1+y*0.45813e-2)))));
       } else {
          y=3.75/ax;
          ans=(exp(ax)/sqrt(ax))*(0.39894228+y*(0.1328592e-1+y*(0.225319e-2+y*(-0.157565e-2+y*(0.916281e-2+y*(-0.2057706e-1+y*(0.2635537e-1+y*(-0.1647633e-1+y*0.392377e-2))))))));
          }
        return ans;
    }
    double bessei1(double x) {
        float ax,ans;
        double y;
            //Accumulate polynomials in double precision.
        if ((ax=fabs(x)) < 3.75) {
            //Polynomial fit.
           y=x/3.75;
           y*=y;
           ans=ax*(0.5+y*(0.87890594+y*(0.51498869+y*(0.15084934+y*(0.2658733e-1+y*(0.301532e-2+y*0.32411e-3))))));
         } else {
           y=3.75/ax;
           ans=0.2282967e-1+y*(-0.2895312e-1+y*(0.1787654e-1-y*0.420059e-2));
           ans=0.39894228+y*(-0.3988024e-1+y*(-0.362018e-2+y*(0.163801e-2+y*(-0.1031555e-1+y*ans))));
           ans *= (exp(ax)/sqrt(ax));
         }
          return x < 0.0 ? -ans : ans;
    }
    double bessei(int n,double x) {
         int j;
         float bi,bim,bip,tox,ans;
         if (n == 0) return bessei0(x);
         if (n == 1) return bessei1(x);
         if (x == 0.0) 
            return 0.0;
         else {
           tox=2.0/fabs(x);
           bip=ans=0.0;
           bi=1.0;
           for (j=2*(n+(int) sqrt(40*n));j>0;j--) {
             //Downward recurrence from even m.
             bim=bip+j*tox*bi;
             bip=bi;
             bi=bim;
             if (fabs(bi) > 1.0e30) {
               //Renormalize to prevent overflows.
               ans *= 1.0e-30;
               bi *= 1.0e-30;
               bip *= 1.0e-30;
              }
          if (j == n) ans=bip;
          }
         ans *= bessei0(x)/bi;
           //Normalize with bessi0.
         return x < 0.0 && (n & 1) ? -ans : ans;
         }
    }
    double cff(double k1,double k2,double lambda,int p) {
       double v,v2;
       double i;
       v=1;
       for(i=1;i<=p;i++) { v*=(p+i)/i;}
       v2=lambda*lambda/(4*k1*k2);
       for(i=1;i<=p;i++) { v*=v2;}
       return v*bessei(p,k1)*bessei(p,k2);
    }
    double cf(double k1,double k2,double lambda) {
        double c=0.0,v;
        int i=0;
        while((v=cff(k1,k2,lambda,i++))>1.0e-100) {c+=v;}
        return (39.478417615*c);
    }
    double intek(double k1,double k2,double lambda) {
	
    }
    double dff(double k1,double k2,double lambda,int p) {
       double v,v2;
       double i;
       v=1;
       for(i=1;i<=p;i++) { v=v*((double) p+i)/i;}
       v2=lambda*lambda/(4*k1*k2);
       for(i=1;i<=p;i++) { v*=v2;}
       return v*(k1*bessei(p+1,k1)*bessei(p,k2)+k2*bessei(p,k1)*bessei(p+1,k2)+2*p*bessei(p,k1)*bessei(p,k2));
    }
    double df(double k1,double k2,double lambda) {
        double d=0.0,v;
        int i=0;
        while((v=dff(k1,k2,lambda,i++))>1.0e-20) {d+=v;}
        return d;
    }
    double bivariate(double f1,double f2,double u1,double u2,double k1,double k2,double lambda) {
        double v1;
        double C;
        v1=k1*cos(f1-u1);
        v1+=k2*cos(f2-u2);
        v1+=lambda*sin(f1-u1)*sin(f2-u2);
        v1=exp(v1);
        C=cf(k1,k2,lambda);
        //printf("v1:%f,C:%f\n",v1,C);
        return v1/C;
    }

}
critcl::cproc test {double k1 double k2 double l } double {
  return log(cf(k1,k2,l))-39.478417615*df(k1,k2,l)/cf(k1,k2,l);
    //  return bessei(20,1);
}
# test: generate data from analytic function
critcl::cproc gendata1 {int num int nvar double* data} ok {
    int i;
    int j;
    double v1,r,p;
    int notaccept; 
    int error;
    error=TCL_OK;
    for (i=0;i<num;i++) {
      for (j=0;j<nvar;j++) {
	 notaccept=1;
         while(notaccept) {
	     v1=(double)rand()/(double)RAND_MAX;
	     v1*=180.0;
	     *(data+nvar*i+j)= v1;
	     p=normaldistr(v1,1,1,0);
	     r=(double)rand()/(double)RAND_MAX;
         if (p < r) {
	     notaccept=1;
	 } else {
	     notaccept=0;
	 }
	 }
      }
    }
    return error;
}

critcl::cproc gendata2 {int num int npair double* phi double* k double* lambda double* data } ok {
    double phi1,phi2,v1,v2,k1,k2,l,p,r;
    int i,j,notaccept;
    int error;
    error=TCL_OK;
    for (i=0;i<num;i++) {
      for (j=0;j<npair;j++) {
	 notaccept=1;
	 phi1=*(phi+j*2);
	 phi2=*(phi+j*2+1);
	 k1=*(k+j*2);
	 k2=*(k+j*2+1);
	 l=*(k+j);
	 //printf("%f,%f,%f\n",phi1,phi2,l);
         while(notaccept) {
	     v1=(double)rand()/(double)RAND_MAX;
	     v1*=360.0;
	     v2=(double)rand()/(double)RAND_MAX;
	     v2*=360.0;
	     *(data+2*npair*i+2*j)= v1;
	     *(data+2*npair*i+2*j+1)=v2;
	     p=bivariate(v1*3.14159265/180,v2*3.14159265/180,phi1,phi2,k1,k2,l);
	     r=(double)rand()/(double)RAND_MAX;
	     //printf("%d ->probability:%f,random:%f\n",i,p,r);
         if (p < r) {
	     notaccept=1;
	 } else {
	     notaccept=0;
	 }
	 }
      }
    } 
    return error;
}

critcl::cproc NN_entropy {int* group int groupsize int kth  int nframe  int nvar double* data } double {
    int i,j,k,k1;
    double d,d0,d1,d2;
    double S,v;
    int iv;
    double* dist= (double*) malloc(kth*nframe*sizeof(double));
    //calculate the distance
    //printf("group:\n");
    //for(k=0;k<groupsize;k++) {
    //   printf("%d,",group[k]);
    //}
    for (i=0;i<nframe;i++) {
      // initialize array for distance rank.			    
      for(k=0;k<kth;k++) {
	 *(dist+i*kth+k)=9.9e199;
       }
       for(j=0;j<nframe;j++) {
         if (i == j) {continue; }
         d=0.0;
         for(k=0;k<groupsize;k++) {
           iv=group[k];
           d0=*(data+i*nvar+iv);
           d1=*(data+j*nvar+iv);
	   //convert the raw data from degrees to radian
           d2=abs(d0-d1)*PI/180;
           while(d2 > 2*PI) {d2=2*PI-d2;}
           d+=d2*d2;
         }
         d=sqrt(d);
	 //prevent overflow
         if (d <= 1.0e-199 ) { continue;}
         //printf("s:%d,d:%f\n",groupsize,d);
	 // rank the distance
         for(k=0;k<kth;k++) {
	   v=*(dist+i*kth+k);
	   if (d < v) {
	     for(k1=kth-1;k1>k;k1--){
	       *(dist+i*kth+k1)=*(dist+i*kth+k1-1);
	     }
	       *(dist+i*kth+k)=d;
	       break;
	   }
         }
         
       }
    }
    S=0.0;
    for(i=0;i<nframe;i++) {
    //printf("%f\n",*(dist+i*kth+kth-1));
    S+=log(*(dist+i*kth+kth-1));
    }
    //printf("S(logR_ik):%f\n",S);
    S*=((double) groupsize)/((double) nframe);
    S+=groupsize*0.572364943;
    S-=gamma(1+((double) groupsize)/2);
    for(i=1;i<=kth-1;i++) { S-=1/i; }
    S+=0.5722;
    S+=log((double) nframe);
    free(dist);
    return S;
}

proc calcentropy { group dihes start wbin nframe nvars} {
    upvar $dihes Dihe
	array unset num
	for { set i 0} { $i < $nframe } { incr i} {
	    set state ""
	    foreach dihe $group {
		append state [expr floor(([lindex $dihe [expr $i*$nvars+$dihe]]-$start)/$wbin)]
		append state ,
	    }
	    if [info exists num($state)] {
		set num($state) [expr $num($state)+1]
	    } else {
		set num($state) 1
	    }
	}
	set s 0.0
	set ptot 0.0
       	foreach state [array names num] {
	    set p [expr double($num($state))/$nframe]
	    set s [expr $s + $p*log($p)]
	    set ptot [expr $ptot +$p]
	}
    return $s
}

proc MIE_hist { group nlevel dihes nframes nvars start wbin} {
    upvar $dihes Dihe
    if { $nlevel > 1} { set S1 [MIE_hist $group [expr $nlevel-1] Dihe $nframes $nvars $start $wbin]} else { set S1 0.0 }
    set groups [select $group $nlevel]
    set S2 0.0
    foreach g $groups {
	set flat [flaten $g]
	set S3 [calcentropy $flat dihes $start $wbin $nframes $nvars]
	set S2 [expr $S2+$S3]
    }
    set S [expr $S1-$S2*((-1)**$nlevel)]
    return $S
}

proc MIE_NN { group nlevel dihes nframes nvars kth} {
    upvar $dihes Dihe
    set c_Dihe [ doublesToByteArray $Dihe]
    if { $nlevel > 1} { set S1 [MIE_NN $group [expr $nlevel-1] Dihe $nframes $nvars $kth]} else {set S1 0.0 }
    set groups [select $group $nlevel]
    set S2 0.0
    foreach g $groups {
	set flat [flaten $g]
	set c_flat [intsToByteArray $flat]
	set S3 [NN_entropy $c_flat [llength $flat] $kth $nframes $nvars $c_Dihe]
	set S2 [expr $S2+$S3]
	#puts "group $g: $S2"
    }
    set S [expr $S1-$S2*((-1)**$nlevel)]
    return $S
}
#list<->bytearray
 proc listToByteArray { valuetype list {elemsize 0} } {
    if { $valuetype == "i" || $valuetype == "I" } {
       if { $::tcl_platform(byteOrder) == "littleEndian" } {
          set valuetype "i"
       } else {
          set valuetype "I"
       }
    }

    switch -- $valuetype {
    f - d - i - I {
       set result [binary format ${valuetype}* $list]
    }
    s {
       set result {}
       foreach elem $list {
          append result [binary format a$elemsize $elem]
       }
    }
    default {
       error "Unknown value type: $valuetype"
    }
    }

    return $result
 }

 interp alias {} stringsToByteArray {} listToByteArray s
 interp alias {} intsToByteArray    {} listToByteArray i
 interp alias {} floatsToByteArray  {} listToByteArray f
 interp alias {} doublesToByteArray {} listToByteArray d

 #
 # Generic routine to convert a bytearray into a list
 #
 proc byteArrayToList { valuetype bytearray {elemsize 0} } {
    if { $valuetype == "i" || $valuetype == "I" } {
       if { $::tcl_platform(byteOrder) == "littleEndian" } {
          set valuetype "i"
       } else {
          set valuetype "I"
       }
    }

    switch -- $valuetype {
    f - d - i - I {
       binary scan $bytearray ${valuetype}* result
    }
    s {
       set result  {}
       set length  [string length $bytearray]
       set noelems [expr {$length/$elemsize}]
       for { set i 0 } { $i < $noelems } { incr i } {
          set elem    [string range $bytearray \
                         [expr {$i*$elemsize}] [expr {($i+1)*$elemsize-1}]]
          set posnull [string first "\000" $elem]
          if { $posnull != -1 } {
             set elem [string range $elem 0 [expr {$posnull-1}]]
          }
          lappend result $elem
       }
    }
    default {
       error "Unknown value type: $valuetype"
    }
    }
    return $result
 }

 interp alias {} byteArrayToStrings {} byteArrayToList s
 interp alias {} byteArrayToInts    {} byteArrayToList i
 interp alias {} byteArrayToFloats  {} byteArrayToList f
 interp alias {} byteArrayToDoubles {} byteArrayToList d


# data structure 
proc select { vlist n } {
    set level(1) $vlist
    for {set l 2} { $l <= $n } {incr l } {
	set level($l) {}
	foreach group $level([expr $l -1]) {
	    set idx [expr 1+[lsearch $vlist [lindex $group end]]]
	    foreach item [lrange $vlist $idx end] {
		if {[lsearch $group $item] < 0} {
		    #puts "$group: $item"
		    lappend level($l) [concat $group $item]
		}
	    }
	}
    }
    return $level($n)
}
proc flaten { list } {
    return $list
}
# time
proc time { block {ntime 1}} {
    puts "Execute: $block"
    set t1 [clock clicks -milliseconds]
    for { set i 0} { $i < $ntime } { incr i } {
	set result [ uplevel 1 $block]
    }
    set t2 [clock clicks -milliseconds]
    set second [expr double($t2-$t1)/(1000*$ntime)]
    #return $second
    puts " in $second seconds\n Result:\n $result"
}



# residue processing

proc groupby { ic filters } {
    upvar $ic IC
    for {set i 0 } { $i < [llength $IC(phi)]} { incr i} {
	set group ""
	foreach f $filters {
	    switch $f {
		residue {
		    set atom [lindex $IC(resid) $i]
		    append group [belongtoResidue $atom]
		    append group ,
		}
		backbone {
		    set atom [lindex $IC(atom) $i ]
		    append group [belongtoBB $atom]
		    append group ,
		}
	    }
	}
	set group [string range $group 0 end-1]
	if [info exists Group($group)] {
	# the following line use number in Charmm IC table 
	#    lappend Group($group) [lindex $IC(num) $i]
	# the following line use array index number for the convient of communicating with C function(for entropy calculation)
	     lappend Group($group) $i
	} else {
	    set Group($group) [list $i]
	}
    }
    return [array get Group]
}
proc belongtoBB { atoms } {
    set BackboneAtom {CA N C O *C *CA *N *O }
    if [contain_only $atoms $BackboneAtom ] { return BB 
    } else {
	return SC
    }
}
proc belongtoResidue { atoms } {
    foreach atom $atoms {
	if [info exists num($atom)] {
	    set num($atom) [expr $num($atom) +1]
	} else {
	    set num($atom) 1
	}
    }
    foreach atom [array names num] {
	if [info exists maxatom] {
	    if { $num($atom) >= $num($maxatom) } {
		set maxatom $atom
	    }
	} else {
	    set maxatom $atom
	}
    }
    return $maxatom
}
proc contain_only { table Set } {
    set only 1
    foreach item $table {
	if { [lsearch $Set $item] <0} {
	    set only 0
	}
    }
    return $only
}
proc count { atomlist } {
    foreach atom $atomlist {
	if [info exists num($atom)] {
	    set num($atom) [expr $num($atom) +1]
	} else {
	    set num($atom) 1
	}
    }
}
